//Simon Mikulcik fall 2011 Team 3843 

/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.camera.*;
import edu.wpi.first.wpilibj.*;


/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the SimpleRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class threadBot extends SimpleRobot {
    BotControls control;
    BotActuators act;
    
    public threadBot(){
        
        control = new BotControls();
        act = new BotActuators(control);
    }
    /**
     * This function is called once each time the robot enters autonomous mode.
     */
    public void autonomous() {
    }

    /**
     * This function is called once each time the robot enters operator control.
     */
    public void operatorControl() {
        
        while(isOperatorControl()){
        Timer.delay(0.005);
            control.println(DriverStationLCD.Line.kMain6, 2, "hi");
        }
    }

class BotControls
{
	DriverStation ds;
	DriverStationLCD dsLCD;
        
	Joystick rightJoy;
	Joystick leftJoy;
        
        BotSensors sensor;
        
	//joystick variables
	int buttonNum;
	boolean joyOutput;
	int lastButton;
	boolean buttonPressed;
        
	public static final int trigger = 1;	
	public static final int low = 4;	
	public static final int middle = 3;	
	public static final int high = 5;	
	public static final int load = 2;	
	public static final int cannon = 7;
	public static final int grip = 6;	
	public static final int deploy = 8;
	public static final int deployControl = 9;
        
        BotControls(){
		// driver station instance for digital I/O
		ds = DriverStation.getInstance();
		dsLCD = DriverStationLCD.getInstance();
                
                sensor = new BotSensors();
		
                rightJoy = new Joystick(1);
		leftJoy = new Joystick(2);
        }
        
        public void println(DriverStationLCD.Line line, int i, String s){
            dsLCD.println(line, i, s);            
        }
}

class BotSensors
{
//gyro
Gyro gyro;
    
    public BotSensors(){
        //gyro
	//gyro = new Gyro(1);
	//gyro.reset();
                
	AxisCamera cam = AxisCamera.getInstance();
	cam.writeResolution(AxisCamera.ResolutionT.k320x240);
    }
    
}

class BotActuators
{
    BotDrive drive;
    BotArm arm;
    Cannon cannon;
    
    public BotActuators(BotControls controls){
        drive = new BotDrive(controls);
        arm = new BotArm(controls);
        cannon = new Cannon(controls);
        go();
    }
    
    private void go(){
        drive.start();
        arm.start();
        cannon.start();
    }
}

class BotDrive extends Thread
{
    RobotDrive myRobot;
    Joystick driveJoy;
    
    public BotDrive(BotControls controls){
        myRobot = new RobotDrive(1, 2);
        driveJoy = controls.leftJoy;
        controls.println(DriverStationLCD.Line.kUser2,1,"hello");
    }
    public void run(){
        while(true){
		myRobot.arcadeDrive(driveJoy);
                Timer.delay(0.005);}
    }
}

class BotArm extends Thread
{
    Joystick armJoy;
    Jaguar mainArmController;
    Victor secondaryArmController;
    Jaguar deployController;
    Gripper grip;
    
    
    DigitalInput limitMainTravelPosition;
    DigitalInput limitSecondaryTravelPosition;
    
    boolean isDefaultArmPosition=false;
    
    public BotArm(BotControls controls){
        armJoy = controls.rightJoy;
	mainArmController = new Jaguar(4,3);
	secondaryArmController = new Victor(4,4);
	deployController = new Jaguar(4,5);
        grip = new Gripper(controls);
        
                                
        limitMainTravelPosition = new DigitalInput(4, 4);
        limitSecondaryTravelPosition = new DigitalInput(4, 5);
    }
    
    public void run(){
        grip.start();
        while(true){
        move();
        Timer.delay(0.005);
        }
    }
    
    int lastButton=0;
    boolean buttonPressed;
    public void readJoystick(){
        buttonPressed = false;
        if(armJoy.getRawButton(BotControls.low) && armJoy.getRawButton(BotControls.trigger)){
            if(lastButton != 1){lastButton = 1;buttonPressed = true; LowArmPosition();}
	}
        else lastButton = 0;
        
    }
    
    
    public void move(){
		mainArmController.set(armJoy.getX(Joystick.Hand.kLeft));			//speed for moving the arm
		secondaryArmController.set(armJoy.getY(Joystick.Hand.kRight));
    }
    
    public void LowArmPosition()
    {
	if(isDefaultArmPosition){DefaultArmPosition();}
	isDefaultArmPosition = false;
	control.println(DriverStationLCD.Line.kUser2,1,"Entering LowArmPosition");
	if(control.sensor.gyro.getAngle() < 68.0){	
		while(armJoy.getRawButton(1) && control.sensor.gyro.getAngle() < 63.0){
			mainArmController.set(1);
			secondaryArmController.set(armJoy.getY(Joystick.Hand.kRight));
			Timer.delay(0.005);
		}
	}else{
		while(1 ==0 && armJoy.getRawButton(1) && control.sensor.gyro.getAngle() > 73.0){
			mainArmController.set(-1);
			secondaryArmController.set(armJoy.getY(Joystick.Hand.kRight));
			Timer.delay(0.005);
		}		
	}
	mainArmController.set(0);
    }
    
    
/**
this zeros out the location of the arms
*/
public void DefaultArmPosition()
{
	grip.close();
	
	isDefaultArmPosition = true;
	//printf("Entering TravelPosition");
	int defaultCompleted=0;
	while(defaultCompleted != 1){
	if(!limitMainTravelPosition.get()){mainArmController.set(-.5);}else{mainArmController.set(0);}
	if(!limitSecondaryTravelPosition.get()){secondaryArmController.set(-.5);}else{secondaryArmController.set(0);}
	if(limitSecondaryTravelPosition.get() && limitMainTravelPosition.get()){defaultCompleted = 1;}	
	Timer.delay(0.005);
	}
	secondaryArmController.set(0);
	mainArmController.set(0);
	
}// end DefaultArmPosition()

}//end BotArm
class Gripper extends Thread
{
        Joystick gripJoy;
        BotControls control;
	Solenoid gripOpenSolenoid;
	Solenoid gripCloseSolenoid;
        boolean gripping;
        Compressor compress;
        
        public Gripper(BotControls controls){
            gripJoy = controls.rightJoy;
            control = controls;
            gripOpenSolenoid = new Solenoid(7, 2);
            gripCloseSolenoid = new Solenoid(7, 1);
            compress = new Compressor(4, 7, 4, 1);
            
            compress.start();
            
            gripping = true;	//is gripper closed
            gripCloseSolenoid.set(true);
            Timer.delay(0.030);
            gripCloseSolenoid.set(false);
            
        }
        
        public void run(){
            while(true){
                if(gripJoy.getRawButton(10)){close();control.println(DriverStationLCD.Line.kUser2,1,"Grip Close!");}
                if(gripJoy.getRawButton(11)){open();control.println(DriverStationLCD.Line.kUser2,1,"Grip Open!");}
            }
        }
        
        public void open(){
		gripping = false;
		gripOpenSolenoid.set(true);
		Timer.delay(0.030);
		gripOpenSolenoid.set(false);
        }
        
        public void close(){
		gripping = true;
		gripCloseSolenoid.set(true);
		Timer.delay(0.030);
		gripCloseSolenoid.set(false);
        }
}

class Cannon extends Thread
{
    Joystick cannonJoy;
    Relay cannonSolenoid;
        
        
    public Cannon(BotControls controls){
                cannonJoy = controls.rightJoy;
		cannonSolenoid = new Relay(4, 4, Relay.Direction.kForward);
		cannonSolenoid.set(Relay.Value.kOff);
    }
        
    public void run(){
        while(true){
        readJoystick();
        Timer.delay(0.01);
        }
    }
    int lastButton=0;
    boolean buttonPressed;
    public void readJoystick(){
        while(cannonJoy.getRawButton(BotControls.trigger)){
                if(cannonJoy.getRawButton(BotControls.cannon) && cannonJoy.getRawButton(BotControls.trigger) && lastButton != BotControls.trigger){
                        lastButton = BotControls.cannon;
                        fire();
                }
                if(!cannonJoy.getRawButton(BotControls.cannon))lastButton = 0;
        }
    }
        public void fire(){
            cannonSolenoid.set(Relay.Value.kOn);
            Timer.delay(0.001 );
            cannonSolenoid.set(Relay.Value.kOff);
        }
}
}//end threadBot
